function output=velocity_err(input)

x=input(1);
y=input(2);

psi=input(3);
x_d=input(4);
y_d=input(5);
%psi_d=input(6);
psi_d=input(7);

x_e=cos(psi)*(x-x_d)+sin(psi)*(y-y_d);
y_e=-sin(psi)*(x-x_d)+cos(psi)*(y-y_d);
psi_e=psi-psi_d;
output=[x_e y_e psi_e];